#ifdef USE_REMOTE_SERIAL

#include <SerialFirmata.h>
#include "kSerialFirmata.h"

#ifdef ARDUINO

kSerialFirmata::kSerialFirmata(mFirmata *_fm, kSerial *_fs, int _id) {
    fm = _fm;
    fs = _fs;
    id = _id;
}

#endif

int kSerialFirmata::begin(unsigned int baud, u8 format, int rx_buff_sz, int tx_buff_sz) {
    u8 function[6];
    data.baud = baud;
    data.config = format;
    if (nullptr == rx_buff)
        rx_buff = Rtos::create_queue(rx_buff_sz);

    function[0] = id | SERIAL_CONFIG;
    *(uint32_t *) &function[1] = baud;
    function[5] = format;
    fm->sendSysex(fs, SERIAL_DATA, 6, (byte *) function, false);
    return 0;
}

int kSerialFirmata::end() {
    u8 function;
    function = id | SERIAL_CLOSE;
    fm->sendSysex(fs, SERIAL_DATA, 1, (byte *) &function, false);
    return 0;
}

int kSerialFirmata::available() {
    u8 function[2];
    if (0 == (flag & IS_OPEN)) {
        begin(data.baud, data.config);
        return 0;
    }
    // function[0] = id | SERIAL_READ;
    // function[1] = SERIAL_READ_CONTINUOUSLY;
    // if ((Rtos::ticks() - next_tick) > 10000)
    // {
    //     fm->sendSysex(fs, SERIAL_DATA, 2, (byte *)function, false);
    //     next_tick = Rtos::ticks();
    // }
    return Rtos::queue_available(rx_buff);
}

int kSerialFirmata::available_wait(int delay) {
    u8 function[2];
    if (0 == (flag & IS_OPEN)) {
        begin(data.baud, data.config);
        return 0;
    }
    function[0] = id | SERIAL_READ;
    function[1] = SERIAL_READ_CONTINUOUSLY;
    if ((Rtos::ticks() - next_tick) > 10000) {
        fm->sendSysex(fs, SERIAL_DATA, 2, (byte *) function, false);
        next_tick = Rtos::ticks();
    }
    return Rtos::queue_wait(rx_buff, delay);
}

int kSerialFirmata::write(uint8_t ch) {
    u8 function[2];
    if ((flag & IS_OPEN) == 0) {
        begin(data.baud, data.config);
        return 0;
    }
    function[0] = id | SERIAL_WRITE;
    function[1] = ch;
    fm->sendSysex(fs, SERIAL_DATA, 2, (byte *) function, false);
    return 1;
}

int kSerialFirmata::write(const u8 *string, int size) {

    if ((flag & IS_OPEN) == 0) {
        begin(data.baud, data.config);
        return 0;
    }
    u8 *dat = (u8 *) malloc(size + 1);
    dat[0] = id | SERIAL_WRITE;
    for (int i = 0; i < size; i++) {
        dat[i + 1] = string[i];
    }
    fm->sendSysex(fs, SERIAL_DATA, size + 1, (byte *) dat, false);
    data.tx_count += size;
    free(dat);
    return size;
}

int kSerialFirmata::read() {
    return Rtos::queue_read(rx_buff, 0);
}

int kSerialFirmata::peek() {

    u8 function;

    if ((flag & IS_OPEN) == 0) {
        begin(data.baud, data.config);
        return 0;
    }
    function = id | SERIAL_FLUSH;
    fm->sendSysex(fs, SERIAL_DATA, 1, (byte *) &function, false);
    return 0;
}

void kSerialFirmata::handleSysex(byte command, byte argc, byte *argv) {
    if (command != SERIAL_DATA)
        return;
    int func = argv[0] & 0xf0;
    switch (func) {
        case SERIAL_REPLY:
            if (flag & IS_OPEN) {
                for (int i = 1; i < argc; i += 1) {
                    u8 c = argv[i];
                    Rtos::queue_write(rx_buff, c);
                }
                data.rx_count += argc - 1;
            }
            break;
        case SERIAL_STATUS:
            if (argv[1] == SERIAL_IS_OPEN)
                flag |= IS_OPEN;
            else if (argv[1] == SERIAL_IS_CLOSE) {
                flag &= ~IS_OPEN;
            }
            break;
        default:
            break;
    }
    next_tick = Rtos::ticks();
}

int kSerialFirmata::read_wait(int timeout) {
    return Rtos::queue_read(rx_buff, timeout);
}

int kSerialFirmata::availableForWrite() {
    return 1;
}

#endif
